/*******************************************************************************
* Copyright 2017 CLNST CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.

*******************************************************************************/
#include <s6h4d_moveit_driver/s6h4d-ctrl.h>

serial::Serial ser;
static unsigned char r_buffer[9];
unsigned char a[48];

/**
  * @brief 
  * @param  
  * @retval 
  */
int s6h4d_chkReset(const double dt[6])
{
     if(
          (dt[J1] == INIT_J1)&&
          (dt[J2] == INIT_J2)&&
          (dt[J3] == INIT_J3)&&
          (dt[J4] == INIT_J4)&&
          (dt[J5] == INIT_J5)&&
          (dt[J6] == INIT_J6)
        )
        {
            return    S6H4D_PLAY;
        }
        else 
        {
            return    S6H4D_RESET;
        }
}

/**
  * @brief 
  * @param  
  * @retval 
  */
double s6h4d_dtMerge(unsigned char x_H, unsigned char x_L)
{
    short tmp = 0;
    double data = 0;
    tmp = x_H;
    tmp <<= 8;
    tmp += x_L;
    data = tmp;
    return data;
}

/**
  * @brief 
  * @param  
  * @retval 
  */
void Float2Byte(float floatNum, unsigned char *byteArry)
{
	unsigned char *pchar = (unsigned char *)&floatNum;
	char i;

	for (i = 0; i < sizeof(float); i++)
	{
		*byteArry = *pchar;
		pchar++;
		byteArry++;
	}
}

/**
  * @brief  机械臂发送角度数据
  * @param  命令，参数，角度值
  * @retval 实际发送的数据
  */
int s6h4d_txSig(unsigned char cmd, const double dt[6])
{
    float xt=(float)dt[J1];
	float yt=(float)dt[J2]; 
	float zt=(float)dt[J3];
	float b0t=(float)dt[J4];
	float b1t=(float)dt[J5];
	float wt=(float)dt[J6];

	float spd=20;
	float at=100;
	float pmw=1500;
	unsigned char ba[4] = { 0,0,0,0 };
	
	unsigned char i_a = cmd;
	unsigned char i_b = 1;

    if(cmd==2)
    {
        spd = 1800;
    }
    else 
    {
        if(xt<-150-INIT_J1)
        {
            xt = -150;
        }
        else if(xt>150-INIT_J1)
        {
            xt = 150;
        }
        xt += INIT_J1; 
        
        if(yt<-65-INIT_J2)
        {
            yt = -65;
        }
        else if(yt>110-INIT_J2)
        {
            yt = 110;
        }
        yt += INIT_J2;

        if(zt<27-INIT_J3)
        {
            zt = 27;
        }
        else if(zt >188-INIT_J3)
        {
            zt = 188;
        }
        zt += INIT_J3;

        if(b0t<-167-INIT_J4)
        {
            b0t = -167;
        }
        else if(b0t>167-INIT_J4)
        {
            b0t = 167;
        }
        b0t += INIT_J4;

        if(b1t<-34-INIT_J5)
        {
            b1t = -34;
        }
        else if(b1t>208-INIT_J5)
        {
            b1t = 208;
        }
        b1t += INIT_J5;

        if(wt<-270-INIT_J6)
        {
            wt = -270;
        }
        else if(wt>270-INIT_J6)
        {
            wt = 270;
        }
        wt += INIT_J6;

        spd = 20;
    }

	a[0] = 238;
	a[1] = i_a + 48;             // 指令a用字符型数据表示
	a[2] = i_b;
	Float2Byte(xt, ba);    
	a[3] = ba[0];
	a[4] = ba[1];
	a[5] = ba[2];
	a[6] = ba[3];
	Float2Byte(yt, ba);
	a[7] = ba[0];
	a[8] = ba[1];
	a[9] = ba[2];
	a[10] = ba[3];
	Float2Byte(zt, ba);
	a[11] = ba[0];
	a[12] = ba[1];
	a[13] = ba[2];
	a[14] = ba[3];

	Float2Byte(b0t, ba); 
	a[15] = ba[0];
	a[16] = ba[1];
	a[17] = ba[2];
	a[18] = ba[3];
	Float2Byte(b1t, ba); 
	a[19] = ba[0];
	a[20] = ba[1];
	a[21] = ba[2];
	a[22] = ba[3];
	Float2Byte(wt, ba);
	a[23] = ba[0];
	a[24] = ba[1];
	a[25] = ba[2];
	a[26] = ba[3];

	Float2Byte(pmw, ba);
	a[27] = ba[0];
	a[28] = ba[1];
	a[29] = ba[2];
	a[30] = ba[3];
	Float2Byte(at, ba);
	a[39] = ba[0];
	a[40] = ba[1];
	a[41] = ba[2];
	a[42] = ba[3];
	Float2Byte(spd, ba);
	a[43] = ba[0];
	a[44] = ba[1];
	a[45] = ba[2];
	a[46] = ba[3];
	a[47] = 239;

    // printf("txd:[ ");
	// for (int i = 0; i < 48; i++)
	// {
	// 	printf("%02x ", a[i]);
	// }
    // printf("]\r\n");

    ser.write(a,48);
    return 1;
}

/**
  * @brief  机械臂接收角度数据
  * @param  接收到的角度, 接收到的末端位姿
  * @retval 机械臂运行状态
  */
int s6h4d_rxSig(double dt[6], double ps[3])
{
    double X,Y,Z;
    if (ser.available()) 
    {
        ser.read(r_buffer, 1);
        if(r_buffer[0]==0xce)
        {
            ser.read(&r_buffer[1], 8);
            if(r_buffer[8]==0xcf)
            {
                // printf("rxd:[  ");
                // for(int k=0; k<9;k++)
                // {
                //     printf("0x%02x ",r_buffer[k]);
                // }
                // printf("]\r\n");
                switch(r_buffer[7])
                {
                    case 0: //坐标值
                        ps[POS_X] = s6h4d_dtMerge(r_buffer[1], r_buffer[2])/10;
                        ps[POS_Y] = s6h4d_dtMerge(r_buffer[3], r_buffer[4])/10;
                        ps[POS_Z] = s6h4d_dtMerge(r_buffer[5], r_buffer[6])/10;
                        break;
                    case 1: //轴角度值1
                        dt[J1] = s6h4d_dtMerge(r_buffer[1], r_buffer[2])/100-INIT_J1;
                        dt[J2] = s6h4d_dtMerge(r_buffer[3], r_buffer[4])/100-INIT_J2;
                        dt[J3] = s6h4d_dtMerge(r_buffer[5], r_buffer[6])/100-INIT_J3;
                        break;
                    case 2: //轴角度值2
                        dt[J4] = s6h4d_dtMerge(r_buffer[1], r_buffer[2])/100-INIT_J4;
                        //dt[J5] = s6h4d_dtMerge(r_buffer[3], r_buffer[4])/100;
                        dt[J6] = s6h4d_dtMerge(r_buffer[5], r_buffer[6])/100-INIT_J6;
                        break;
                    // case 3: //工作台坐标
                    //     break;
                    // case 4: //状态信息1
                    //     break;
                    case 5: //状态信息2
                        dt[J5] = s6h4d_dtMerge(r_buffer[3], r_buffer[4])/100-INIT_J5;
                        break;
                    // case 100:   //状态信息3
                    //     break;
                    // case 101:   //状态信息4
                    //     break;
                    // case 102:   //状态信息5
                    //     break;
                    // case 103:   //状态信息6
                    //     break;
                    // case 105:   //状态信息7
                        break;
                    default:
                        break;
                }
            } 
        }
    }
    return 1;
}

/**
  * @brief  初始化机械臂连接
  * @param  无
  * @retval 初始化结果
  */
int s6h4d_init(const char * dev)
{
    try 
    { 
    //设置串口属性，并打开串口 
        ser.setPort(dev); 
        ser.setBaudrate(9600); 
        serial::Timeout to = serial::Timeout::simpleTimeout(1000); 
        ser.setTimeout(to); 
        ser.open(); 
    } 
    catch (serial::IOException& e) 
    { 
        //ROS_ERROR_STREAM("Unable to open port "); 
        printf("unable to open port.\r\n");
        return -1; 
    }
    printf("开启机械臂通信\r\n");
    return 1;
}

/**
  * @brief 断开机械臂连接
  * @param  无
  * @retval 无
  */
void s6h4d_deinit(void)
{
  printf("关闭机械臂通信\r\n");
  if(ser.isOpen())
  {
    ser.close();
  }
}

/************************ (C) COPYRIGHT HopeMotion *****END OF FILE****/

